# Code snarfed from http://arainyday.se/projects/python/AStar/

# Author has put it in the Public Domain.

# Version 1.1
#
# Changes in 1.1:
# In order to optimize the list handling I implemented the location id (lid) attribute.
# This will make the all list serahces to become extremely more optimized.

class Path:
    def __init__(self,nodes, totalCost):
        self.nodes = nodes
        self.totalCost = totalCost

    def __cmp__(self, other):
        return cmp(self.totalCost, other.totalCost)

    def __repr__(self):
        return '<Path of len=%d cost=%g>'%(len(self.nodes), self.totalCost)

class Node:
    def __init__(self,location,nCost,lid,parent=None):
        self.location = location # where is this node located
        self.nCost = nCost       # cost it was to move to this node
        self.mCost = nCost       # total move cost to reach this node
                                 # is modified after initialisation
        self.parent = parent     # parent node
        self.score = 0           # calculated score for this node (mCost + nCost)
        self.lid = lid           # set the location id - unique for each location in the map

    def __eq__(self, n):
        if n.lid == self.lid:
            return 1
        else:
            return 0

    def __repr__(self):
        return '<Node %d,%d>'%self.location

class AStar:

    def __init__(self,maphandler):
        self.mh = maphandler

    def _getBestOpenNode(self):
        bestNode = None
        for n in self.on:
            if not bestNode:
                bestNode = n
            else:
                if n.score<=bestNode.score:
                    bestNode = n
        return bestNode

    def _tracePath(self,n):
        nodes = []
        totalCost = n.mCost
        p = n.parent
        nodes.insert(0,n)

        while 1:
            if p.parent is None:
                break

            nodes.insert(0,p)
            p=p.parent

        return Path(nodes,totalCost)

    def _handleNode(self,node,end):
        i = self.o.index(node.lid)
        self.on.pop(i)
        self.o.pop(i)
        self.c.append(node.lid)

        nodes = self.mh.getAdjacentNodes(node,end)
        for n in nodes:
            if n.location == end:
                # reached the destination
                return n
            elif n.lid in self.c:
                # already in close, skip this
                continue
            elif n.lid in self.o:
                # already in open, check if better score
                i = self.o.index(n.lid)
                on = self.on[i];
                if n.mCost<on.mCost:
                    self.on.pop(i);
                    self.o.pop(i);
                    self.on.append(n);
                    self.o.append(n.lid);
            else:
                # new node, append to open list
                self.on.append(n);
                self.o.append(n.lid);

        return None

    def findPath(self, fromlocation, tolocation, close_enough=False):
        self.o = []
        self.on = []
        self.c = []

        fnode = self.mh.getNode(fromlocation)
        self.on.append(fnode)
        self.o.append(fnode.lid)
        nextNode = fnode
        #print 'FIND', fromlocation, tolocation

        lastNode = None
        while nextNode is not None:
            #print 'looking in', nextNode.location
            finish = self._handleNode(nextNode, tolocation)
            if finish:
                return self._tracePath(finish)
            lastNode = nextNode
            nextNode = self._getBestOpenNode()

        # if close enough is good enough, and we've actually moved somewhere
        # then trace a path as far as we got
        if close_enough and lastNode != fnode:
            return self._tracePath(lastNode)

        return None

class SQ_Location:
    """A simple Square Map Location implementation"""
    def __init__(self,x,y):
        self.x = x
        self.y = y

    def __eq__(self, l):
        """MUST BE IMPLEMENTED"""
        if l.x == self.x and l.y == self.y:
            return 1
        else:
            return 0

class SQ_MapHandler:
    """A simple Square Map implementation"""

    def __init__(self,mapdata,width,height):
        self.m = mapdata
        self.w = width
        self.h = height

    def getNode(self, location):
        """MUST BE IMPLEMENTED"""
        x = location.x
        y = location.y
        if x<0 or x>=self.w or y<0 or y>=self.h:
            return None
        d = self.m[(y*self.w)+x]
        if d == -1:
            return None

        return Node(location,d,((y*self.w)+x));

    def getAdjacentNodes(self, curnode, dest):
        """MUST BE IMPLEMENTED"""
        result = []

        cl = curnode.location
        dl = dest

        n = self._handleNode(cl.x+1, cl.y, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x-1, cl.y, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x, cl.y+1, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x, cl.y-1, curnode, dl.x, dl.y)
        if n: result.append(n)

        n = self._handleNode(cl.x+1, cl.y+1, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x+1, cl.y-1, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x-1, cl.y+1, curnode, dl.x, dl.y)
        if n: result.append(n)
        n = self._handleNode(cl.x-1, cl.y-1, curnode, dl.x, dl.y)
        if n: result.append(n)

        return result

    def _handleNode(self,x,y,fromnode,destx,desty):
        n = self.getNode(SQ_Location(x,y))
        if n is not None:
            dx = abs(x - destx)
            dy = abs(y - desty)
            n.nCost = max(dx, dy)
            n.mCost += fromnode.mCost
            n.score = n.mCost + n.nCost
            n.parent=fromnode
            return n

        return None

